#include <lib/bitmap.h>
#include <lib/stddef.h>
#include <lib/string.h>
#include<lib/assert.h>

// init a bitmap struct
void init_bitmap(bitmap_t *bitmap)
{
    assert(bitmap);
    assert(bitmap->bits!=NULL);

    memset(bitmap->bits, 0, bitmap->length);
}

// test bits
bool bitmap_test(bitmap_t *bitmap, uint32_t index)
{
    uint32_t byte_idx = index / 8;
    uint32_t byte_off = index % 8;

    assert(bitmap);
    assert(bitmap->bits!=NULL);

    return bitmap->bits[byte_idx] & (1 << byte_off);
}

// set bitmap assume bits to value
void bitmap_set(bitmap_t *bitmap, uint8_t value, uint32_t index)
{
    uint32_t byte_index = index / 8;
    uint32_t byte_off = index % 8;

    assert(bitmap);
    assert(bitmap->bits);

    // set assume bit to 1
    if (value)
        bitmap->bits[byte_index] = bitmap->bits[byte_index] | (1 << byte_off);
    else
        // set assume bit to 0
        bitmap->bits[byte_index] = bitmap->bits[byte_index] & (~(1 << byte_off));
}

// traversal assume bytes from assume position
void bitmap_traversal(bitmap_t *bitmap, uint32_t len, uint32_t pos)
{
    int i = 0;

    if(!bitmap)
        return ;

    // if len above bitmap actual len,we only traversal actual part
    if (len > bitmap->length)
        len = bitmap->length;

    for (i = 0; i < len; i++)
    {
        // from pos start get bits value
        bitmap_findbyte(bitmap, pos + i);
    }
}

// find assume bits
bool bitmap_find(bitmap_t *bitmap, uint32_t index)
{
    uint32_t byte_index = index / 8;
    uint32_t byte_off = index % 8;

    // if index above limit
    if (byte_index < bitmap->length)
        return bitmap->bits[byte_index] & (1 << byte_off);
    else
        return -1;
}

// find assume byte
uint8_t bitmap_findbyte(bitmap_t *bitmap, uint32_t index)
{
    assert(index>bitmap->length);
    return bitmap->bits[index];
}

// get used bits count
uint32_t bitmap_used_count(bitmap_t *bitmap)
{
    uint32_t count;
    int i;

    for (i = 0; i < BITS_PER_BYTE * bitmap->length; i++)
    {
        if (bitmap_test(bitmap, i))
            count++;
    }

    return count;
}

// get unused bits count
uint32_t bitmap_unused_count(bitmap_t *bitmap)
{
    uint32_t count;
    int i;

    for (i = 0; i < BITS_PER_BYTE * bitmap->length; i++)
    {
        if (!bitmap_test(bitmap, i))
            count++;
    }

    return count;
}

// bitmap find free bits
int bitmap_find_free(bitmap_t *bitmap)
{
    int i;

    for (i = 0; i < BITS_PER_BYTE * bitmap->length; i++)
    {
        if (!bitmap_test(bitmap, i))
            return i;
        else
            continue;
    }

    return -1;
}

int bitmap_find_nfree(bitmap_t *bitmap, uint64_t len)
{
   unsigned long idx_byte = 0;

   while (( 0xff == bitmap->bits[idx_byte]) && (idx_byte < bitmap->length)) {
      idx_byte++;
   }

   
   if (idx_byte == bitmap->length) {  
      return -1;
   }

   long idx_bit = 0;

   while ((unsigned char)(1 << idx_bit) & bitmap->bits[idx_byte]) { 
	 idx_bit++;
   }
	 
   long idx_start = idx_byte * 8 + idx_bit;  
   if (len== 1) {
      return idx_start;
   }

   unsigned long bit_left = (bitmap->length * 8 - idx_start);
   unsigned long next_bit = idx_start + 1;
   unsigned long count = 1;

   idx_start = -1;
   while (bit_left-- > 0) {
      if (!(bitmap_test(bitmap, next_bit))) {
	 count++;
      } else {
	 count = 0;
      }
      if (count == len) {
	 idx_start = next_bit - len + 1;
	 break;
      }
      next_bit++;          
   }
   return idx_start;
}
